//----------------------------------------------------------------------------------------------
//	Filename:	FeatureDescriptorStructureGraph.h
//	Author:		Sandro Spina
//	Date:		23/07/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#pragma once

#include "UniqueId.h"
#include <map>
#include <Geometry\Vector3.h>
#include <Geometry\Basis.h>
#include <Shape\CloudPoint.h>
#include "SegmentsGraph.h"
#include "StructureGraph.h"

namespace Illumina
{
	namespace Core
	{
		class FeatureDescStructureGraph
		{
		public:							

			//More actions can be added here eventually ...
			enum action {add_direction,transition_count};

			enum attributes {
				angle,			//angle between nodes (shapes)
				dot				//dot product between nodes (shapes)
			};						
			
			//Node IDs are the same as corresponding surface segment ids.
			struct EdgeStats
			{
				int strength;
				float dot;
				float epsilon;	// +/- epsilon from dot
				float direct;	//Normalised direct connection value (dependant on scans in training set)
				float occluded; //Normalised occlusion value (dependant on scans in training set)
			    bool disconnected; //High Imporance Nodes added to this node's edges which are not directly connected. Defaults to false.				
				std::vector<action> actions;

				EdgeStats()
					: strength(-1)
					, dot(0.5f)
					, epsilon(0.1f)
					, direct(1.0f)
					, occluded(0.5f)
					, disconnected(false)
				{ }

				int sizeInBytes()
				{
					int s = 0;
					s += sizeof(int);		//strength
					s += sizeof(float);		//dot
					s += sizeof(float);		//epsilon
					s += sizeof(float);		//direct
					s += sizeof(float);		//occluded
					s += sizeof(bool);		//disconnected
					s += sizeof(int) + (actions.size() * sizeof(action));
					return s;
				}

			};

			struct Node
			{
				int id;	
				int used;
				float coverage;
				std::pair<float, float> distances_over_under;
				int point_count;					//Computed from Training Set
				SegmentsGraph::Shape shape;			//Derived from Training Set	
				Vector3 orientation;				//Derived from Training Set
				SegmentsGraph::OBB obb;				//Computed from Training Set - represents obb of surface segment
				std::map<int, FeatureDescStructureGraph::EdgeStats*>* edges;  //n_src---estats--->n_sink <sink node id, edge states>

				int sizeInBytes()
				{
					int s = 0;
					s += sizeof(id);									//id
					s += sizeof(used);									//used
					s += sizeof(coverage);								//coverage
					s += sizeof(float) * 2;								//distances_over_under
					s += sizeof(point_count);							//point_count
					s += sizeof(shape);									//shape
					s += sizeof(float) * 3;								//orienatation
					s += obb.sizeInBytes();								//size of OBB
					s += sizeof(int);					
					for (auto ed : *edges) 
					{
						s += sizeof(int);						
						s += ed.second->sizeInBytes();
					}
					return s;
				}

				Node()
					: used(0)
					, coverage(0.f)
					, edges(new std::map<int, FeatureDescStructureGraph::EdgeStats*>())
				{ }

				~Node()
				{
					delete edges;					
				}
			};			

			struct VoxelGrid
			{	
				struct VoxelGridCell
				{
					int u_pos;
					int v_pos;
					int w_pos;

					int surface_points_count;
					int edge_points_count;
					int segId;
					bool inOrigin;
					bool active;
					bool inModel;

					VoxelGridCell(VoxelGridCell &p_vgc)
					{
						u_pos = p_vgc.u_pos;
						v_pos = p_vgc.v_pos;
						w_pos = p_vgc.w_pos;
						surface_points_count = p_vgc.surface_points_count;
						edge_points_count = p_vgc.edge_points_count;
						inOrigin = p_vgc.inOrigin;
						active = p_vgc.active;
						inModel = p_vgc.inModel;
						segId = p_vgc.segId;
					}

					VoxelGridCell(void) { };

					inline void Set(int p_u_pos, int p_v_pos, int p_w_pos, int p_surface_points_count, int p_edge_points_count, bool _originCell, int p_segId) {
						u_pos = p_u_pos;
						v_pos = p_v_pos;
						w_pos = p_w_pos;

						surface_points_count = p_surface_points_count;
						edge_points_count = p_edge_points_count;
						inOrigin = _originCell;
						segId = p_segId;
						active = false;
						inModel = false;						
					}

					inline void IncSPC(bool _originCell)
					{
						surface_points_count++;
						inOrigin |= _originCell;
					}

					inline void IncEPC()
					{
						edge_points_count++;						
					}

					int sizeInBytes()
					{
						return sizeof(int) + sizeof(int) + sizeof(int) + sizeof(int) + sizeof(int) + sizeof(bool);
					}

				};

				struct Match_Result {
					float coverage;
					int matches;

					int in_anchor_model;
					int in_anchor_scene;

					int not_in_anchor_model;
					int not_in_anchor_scene;

					int anchor_matches;
					int not_in_anchor_matches;
					int new_cells;

					float inclusion_bias;
					float anchor_match_bias;
					float non_anchor_match_bias;
					float segment_inclusion_bias;

					std::map<int, int> segments_used;

					Match_Result(void) 
					{ 
						coverage = 0.f; 
						matches = 0; 
						in_anchor_model = 0; 
						in_anchor_scene = 0; 
						not_in_anchor_model = 0; 
						not_in_anchor_scene = 0; 
						anchor_matches = 0;
						not_in_anchor_matches = 0;
						inclusion_bias = 0.f;
						anchor_match_bias = 0.f;
						non_anchor_match_bias = 0.f;
						segment_inclusion_bias = 0.f;
						new_cells = 0;
						segments_used = std::map<int,int>();
					};

					void Reset() 
					{ 
						coverage = 0.f; 
						matches = 0; 
						in_anchor_model = 0; 
						in_anchor_scene = 0; 
						not_in_anchor_model = 0; 
						not_in_anchor_scene = 0; 
						anchor_matches = 0;
						not_in_anchor_matches = 0;
						inclusion_bias = 0.f;
						anchor_match_bias = 0.f;
						non_anchor_match_bias = 0.f;
						segment_inclusion_bias = 0.f;
						new_cells = 0;
						segments_used.clear();
					};

				};

				//static int const max_number_cells = 1000;
				std::map<int, VoxelGridCell> * m_voxels;
				FeatureDescStructureGraph * m_parent;				
				int points_total;

				int idx;

				OrthonormalBasis uvw;		//Grid Comparisons are carried out here				
				Vector3 o_model_xyz;		//Origin in world space of the grid. This is in XYZ coordinate space.

				int o_grid_u;			//Origin in grid space
				int o_grid_v;			//Origin in grid space
				int o_grid_w;			//Origin in grid space
				
				Vector3 min_model_uvw;	//Min coordinates (Model) in the UVW basis
				Vector3 max_model_uvw;	//Max coordinates (Model) in the UVW basis

				Vector3 min_obb_uvw;	//Min coordinates (OBB) in the UVW basis
				Vector3 max_obb_uvw;	//Max coordinates (OBB) in the UVW basis

				Vector3 min_model_uvw_br; //min_model uvw before rotation is applied
				
				Vector3 scale;
				float rotation;			 

				int above_u_max;
				int below_u_min;
				int above_v_max;
				int below_v_min;
				int above_w_max;
				int below_w_min;

				int v;
				int dist_above_u_max;
				int dist_below_u_min;
				int dist_above_v_max;
				int dist_below_v_min;
				int dist_above_w_max;
				int dist_below_w_min;
				
				float grid_cell_spacing_u;
				float grid_cell_spacing_v;
				float grid_cell_spacing_w;

				int u_cells_count;
				int v_cells_count;
				int w_cells_count;
				
				bool m_inverse_u;

				std::map<int, VoxelGridCell>::iterator active_voxel;

				inline static int GetKey(int x, int y, int z)	{ return ((x&1023)<<20) | ((y&1023)<<10) | z&1023; }
				inline static int GetXFromKey(int _key)			{ return (_key >> 20) & 1023;	}
				inline static int GetYFromKey(int _key)			{ return (_key >> 10) & 1023;	}
				inline static int GetZFromKey(int _key)			{ return (_key) & 1023;			}

				inline void IncVoxelSurfaceCount(int p_u, int p_v, int p_w, bool p_origin, int p_segId)
				{					
					int key = VoxelGrid::GetKey(p_u, p_v, p_w);
					auto voxels_it = m_voxels->find(key);
					if (voxels_it == m_voxels->end()) 
					{ 
						(*m_voxels)[key].Set(p_u, p_v, p_w, 1, 0, p_origin, p_segId); 
						active_voxel = m_voxels->find(key); 
					} 
					else
					{
						voxels_it->second.IncSPC(p_origin);
						active_voxel = voxels_it;
					}

					points_total++;					
				}

				inline void IncVoxelEdgeCount(int p_u, int p_v, int p_w, int p_segId)
				{
					int key = VoxelGrid::GetKey(p_u, p_v, p_w);
					auto voxels_it = m_voxels->find(key);
					if (voxels_it == m_voxels->end()) 
					{ 
						//std::cout << "Adding new voxel:: " << p_u << " " << p_v << " " << p_w << std::endl;
						(*m_voxels)[key].Set(p_u, p_v, p_w, 0, 1, false, p_segId); 
						active_voxel = m_voxels->find(key); 
					} 
					else
					{
						voxels_it->second.IncEPC();
						active_voxel = voxels_it;
					}

					points_total++;	
				}

				inline void IncActiveVoxelSurfaceCount(bool p_origin)
				{
					active_voxel->second.IncSPC(p_origin);				
				}	

				inline void IncActiveVoxelEdgeCount()
				{
					active_voxel->second.IncEPC();				
				}

				std::vector<std::pair<std::pair<bool, float>, Vector3>> * GetActiveOverlappingCells(VoxelGrid * _targetGrid)
				{
					std::vector<std::pair<std::pair<bool, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<bool, float>, Vector3>>();
					
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();					
					Vector3 min_obb_xyz; Vector3 max_obb_xyz;
					Matrix3x3::Product(uvw_inverse, min_obb_uvw, min_obb_xyz);
					Matrix3x3::Product(uvw_inverse, max_obb_uvw, max_obb_xyz);
					min_obb_xyz+=o_model_xyz;
					max_obb_xyz+=o_model_xyz;

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					float radius = grid_cell_spacing_u/8;
					
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), o_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), min_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(true, grid_cell_spacing_u/2), max_model_xyz));					
				
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(uvw.U, rotation);

					Matrix3x3 rot_projection = yz_rotation * uvw_inverse;
						
					int discarded_spheres = 0;
					Vector3 g_pos_rot;
					Vector3 g_pos_xyz;
					for (int u=0; u<u_cells_count; u++)
					{						
						for (int v=0; v<v_cells_count; v++)
						{
							for (int w=0; w<w_cells_count; w++)
							{								
								auto voxel = m_voxels->find(GetKey(u,v,w));
								if (voxel != m_voxels->end()) 
								{
									Vector3 grid_xyz(	u*grid_cell_spacing_u+(min_model_uvw_br.X),
														v*grid_cell_spacing_v+(min_model_uvw_br.Y),
														w*grid_cell_spacing_w+(min_model_uvw_br.Z));	

									std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
									if (voxel != m_voxels->end())
										if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

									if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

									Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

									g_pos_xyz+=o_model_xyz;
									activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));
								} else
								{									
									discarded_spheres ++;
								}
							}
						}
					}
					std::cout << "Discarded Spheres: " << discarded_spheres << std::endl;
					return activeCellsLocations;
				}

				std::vector<std::pair<std::pair<bool, float>, Vector3>> * GetAllCellsShowingActive(VoxelGrid * _targetGrid) 
				{
					std::vector<std::pair<std::pair<bool, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<bool, float>, Vector3>>();
					
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = _targetGrid->uvw.GetMatrix();
					uvw_inverse.Transpose();					
					Vector3 min_obb_xyz; Vector3 max_obb_xyz;
					Matrix3x3::Product(uvw_inverse, _targetGrid->min_obb_uvw, min_obb_xyz);
					Matrix3x3::Product(uvw_inverse, _targetGrid->max_obb_uvw, max_obb_xyz);
					min_obb_xyz+=_targetGrid->o_model_xyz;
					max_obb_xyz+=_targetGrid->o_model_xyz;

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, _targetGrid->min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, _targetGrid->max_model_uvw, max_model_xyz);
					min_model_xyz+=_targetGrid->o_model_xyz;
					max_model_xyz+=_targetGrid->o_model_xyz;
					
					float radius = grid_cell_spacing_u/2;
					
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(0, grid_cell_spacing_u/2), _targetGrid->o_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(0, 0.5), min_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(1, 0.9), max_model_xyz));					
				
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(_targetGrid->uvw.U, _targetGrid->rotation);

					Matrix3x3 rot_projection = yz_rotation * uvw_inverse;
						
					int discarded_spheres = 0;
					Vector3 g_pos_rot;
					Vector3 g_pos_xyz;		
					for (auto active_cells : *m_voxels)
					{						
						Vector3 grid_xyz(GetXFromKey(active_cells.first)*_targetGrid->grid_cell_spacing_u+(_targetGrid->min_model_uvw_br.X),
										GetYFromKey(active_cells.first)*_targetGrid->grid_cell_spacing_v+(_targetGrid->min_model_uvw_br.Y),
										GetZFromKey(active_cells.first)*_targetGrid->grid_cell_spacing_w+(_targetGrid->min_model_uvw_br.Z));	
						std::pair<bool, float> r = std::pair<bool, float>(true, radius);												
						(_targetGrid->m_voxels->find(active_cells.first) == _targetGrid->m_voxels->end()) ? r.first=true : r.first=false;
						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);
						g_pos_xyz+=_targetGrid->o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));												
					}												
					return activeCellsLocations;
				}

				std::vector<std::pair<std::pair<int, float>, Vector3>> * GetActiveCells_GridView()
				{
					std::vector<std::pair<std::pair<int, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<int, float>, Vector3>>();
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					float radius = grid_cell_spacing_u/4;
					//Include in point set centre, min and max of OBB
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(0, 0.3f), o_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(1, 0.3f), min_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(2, 0.3f), max_model_xyz));									
					
					//Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);					
					//Vector3 mm_uvw; Matrix3x3::Product(yz_rotation_i, min_model_uvw, mm_uvw);					
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, rotation);
					Vector3 mmuvw; Matrix3x3::Product(yz_rotation, min_model_uvw_br, mmuvw);
					float diff = Maths::FAbs(mmuvw.X - min_model_uvw.X) + Maths::FAbs(mmuvw.Y - min_model_uvw.Y) + Maths::FAbs(mmuvw.Z - min_model_uvw.Z);
					if (diff > 0.001f) 
					{
						//std::cout << "Not Equal !! Computed MMUVW : " << mmuvw.ToString() << " Stored MMUVW : " << min_model_uvw.ToString() << std::endl;
						yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);
					}

					Matrix3x3 rot_projection = uvw_inverse * yz_rotation;										

					Vector3 centreShift = Vector3(grid_cell_spacing_u/2, grid_cell_spacing_v/2, grid_cell_spacing_w/2);
					Vector3 topShift = Vector3(grid_cell_spacing_u, grid_cell_spacing_v, grid_cell_spacing_w);

					int discarded_spheres = 0;					
					Vector3 g_pos_xyz;							
					float sphere_spacing = 0.1f;
					int sphere_count_u = (int)((float)grid_cell_spacing_u / sphere_spacing);
					int sphere_count_v = (int)((float)grid_cell_spacing_v / sphere_spacing);
					int sphere_count_w = (int)((float)grid_cell_spacing_w / sphere_spacing);

					for (auto voxels_it = m_voxels->begin(); voxels_it != m_voxels->end(); voxels_it++)
					{								

						Vector3 grid_xyz(	(GetXFromKey(voxels_it->first))*grid_cell_spacing_u+(min_model_uvw_br.X),
											(GetYFromKey(voxels_it->first))*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(GetZFromKey(voxels_it->first))*grid_cell_spacing_w+(min_model_uvw_br.Z));

						Vector3 grid_xyz_top((GetXFromKey(voxels_it->first))*grid_cell_spacing_u+(min_model_uvw_br.X),
											(GetYFromKey(voxels_it->first))*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(GetZFromKey(voxels_it->first))*grid_cell_spacing_w+(min_model_uvw_br.Z));
						grid_xyz_top+=topShift;
						float sphere_radius = 0.05f;

						if (voxels_it->second.active)
						{
							for (int i = 0; i<sphere_count_u; i++)
							{
								Vector3 point = Vector3(grid_xyz.X+(sphere_spacing*i), grid_xyz.Y, grid_xyz.Z);								
								Matrix3x3::Product(rot_projection, point, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r = std::pair<int, float>(1, sphere_radius);
								if (i==0) r.second = sphere_radius*1.5;
								activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

								Vector3 point2 = Vector3(grid_xyz_top.X-(sphere_spacing*i), grid_xyz_top.Y, grid_xyz_top.Z);								
								Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r2 = std::pair<int, float>(1, sphere_radius);
								if (i==0) r2.second = sphere_radius*1.5;
								activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
							}

							for (int i = 1; i<sphere_count_v; i++)
							{
								Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y+(sphere_spacing*i), grid_xyz.Z);								
								Matrix3x3::Product(rot_projection, point, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r = std::pair<int, float>(1, sphere_radius);
								if (i==0) r.second = sphere_radius*1.5f;
								activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

								Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y-(sphere_spacing*i), grid_xyz_top.Z);								
								Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r2 = std::pair<int, float>(1, sphere_radius);
								if (i==0) r2.second = sphere_radius*1.5f;
								activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
							}

							for (int i = 1; i<sphere_count_w; i++)
							{
								Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y, grid_xyz.Z+(sphere_spacing*i));								
								Matrix3x3::Product(rot_projection, point, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r = std::pair<int, float>(1, sphere_radius);
								if (i==0) r.second = sphere_radius*1.5f;
								activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

								Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y, grid_xyz_top.Z-(sphere_spacing*i));	
								Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r2 = std::pair<int, float>(1, sphere_radius);
								if (i==0) r2.second = sphere_radius*1.5f;
								activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));								
							}
						}
						else
						{
							if (voxels_it->second.inModel)
							{
								/*
								for (int i = 0; i<sphere_count_u; i++)
								{
									Vector3 point = Vector3(grid_xyz.X+(sphere_spacing*i), grid_xyz.Y, grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									//Vector3 point2 = Vector3(grid_xyz_top.X-(sphere_spacing*i), grid_xyz_top.Y, grid_xyz_top.Z);								
									//Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									//g_pos_xyz+=o_model_xyz;								
									//std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									//if (i==0) r2.second = 0.25f;
									//p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
									
								}

								for (int i = 1; i<sphere_count_v; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y+(sphere_spacing*i), grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									//Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y-(sphere_spacing*i), grid_xyz_top.Z);								
									//Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									//g_pos_xyz+=o_model_xyz;								
									//std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									//if (i==0) r2.second = 0.25f;
									//p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));									
								}

								for (int i = 1; i<sphere_count_w; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y, grid_xyz.Z+(sphere_spacing*i));								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									//Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y, grid_xyz_top.Z-(sphere_spacing*i));	
									//Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									//g_pos_xyz+=o_model_xyz;								
									//std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									//if (i==0) r2.second = 0.25f;
									//p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));								
									
								}*/
							}
							else
							{
								for (int i = 0; i<sphere_count_u; i++)
								{
									Vector3 point = Vector3(grid_xyz.X+(sphere_spacing*i), grid_xyz.Y, grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, sphere_radius);
									if (i==0) r.second = sphere_radius*1.5f;
									activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									Vector3 point2 = Vector3(grid_xyz_top.X-(sphere_spacing*i), grid_xyz_top.Y, grid_xyz_top.Z);								
									Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r2 = std::pair<int, float>(2, sphere_radius);
									if (i==0) r2.second = sphere_radius*1.5f;
									activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
									
								}

								for (int i = 1; i<sphere_count_v; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y+(sphere_spacing*i), grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, sphere_radius);
									if (i==0) r.second = sphere_radius*1.5f;
									activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y-(sphere_spacing*i), grid_xyz_top.Z);								
									Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r2 = std::pair<int, float>(2, sphere_radius);
									if (i==0) r2.second = sphere_radius*1.5f;
									activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
									
								}

								for (int i = 1; i<sphere_count_w; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y, grid_xyz.Z+(sphere_spacing*i));								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, sphere_radius);
									if (i==0) r.second = sphere_radius*1.5f;
									activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y, grid_xyz_top.Z-(sphere_spacing*i));	
									Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r2 = std::pair<int, float>(2, sphere_radius);
									if (i==0) r2.second = sphere_radius*1.5f;
									activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));									
								}
							}
						}						
					}
										
					return activeCellsLocations;
				}				    

				std::vector<std::pair<std::pair<int, float>, Vector3>> * GetActiveCells()
				{
					std::vector<std::pair<std::pair<int, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<int, float>, Vector3>>();
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					float radius = grid_cell_spacing_u/4;
					//Include in point set centre, min and max of OBB
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(0, 0.5f), o_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(1, 0.2f), min_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(2, 0.8f), max_model_xyz));									
					
					//Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);					
					//Vector3 mm_uvw; Matrix3x3::Product(yz_rotation_i, min_model_uvw, mm_uvw);					
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, rotation);
					Vector3 mmuvw; Matrix3x3::Product(yz_rotation, min_model_uvw_br, mmuvw);
					float diff = Maths::FAbs(mmuvw.X - min_model_uvw.X) + Maths::FAbs(mmuvw.Y - min_model_uvw.Y) + Maths::FAbs(mmuvw.Z - min_model_uvw.Z);
					if (diff > 0.001f) 
					{
						//std::cout << "Not Equal !! Computed MMUVW : " << mmuvw.ToString() << " Stored MMUVW : " << min_model_uvw.ToString() << std::endl;
						yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);
					}

					Matrix3x3 rot_projection = uvw_inverse * yz_rotation;										

					int discarded_spheres = 0;					
					Vector3 g_pos_xyz;							
					for (auto voxels_it = m_voxels->begin(); voxels_it != m_voxels->end(); voxels_it++)
					{								
						Vector3 grid_xyz(	(GetXFromKey(voxels_it->first))*grid_cell_spacing_u+(min_model_uvw_br.X),
											(GetYFromKey(voxels_it->first))*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(GetZFromKey(voxels_it->first))*grid_cell_spacing_w+(min_model_uvw_br.Z));

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);
						g_pos_xyz+=o_model_xyz;
						
						std::pair<int, float> r = std::pair<int, float>(1, 0.2f);
						
						if (voxels_it->second.active) r.second = 0.5f;
						if (voxels_it->second.inModel) r.first = 2;
						activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));				
					}
										
					return activeCellsLocations;
				}

				inline int GetVoxelCountInOrigin()
				{
					int inOrigin = 0;
					for (auto voxels_it = m_voxels->begin(); voxels_it != m_voxels->end(); voxels_it++)
					{
						if (voxels_it->second.inOrigin) inOrigin++;
					}
					return inOrigin;
				}

				void SetActiveCells(std::vector<std::pair<std::pair<int, float>, Vector3>> &p_activeCellLocations)
				{
					p_activeCellLocations.clear();
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					//float radius = grid_cell_spacing_u/4;
					//Include in point set centre, min and max of OBB
					p_activeCellLocations.push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(0, 0.5f), o_model_xyz));
					p_activeCellLocations.push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(1, 0.2f), min_model_xyz));
					p_activeCellLocations.push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(2, 0.8f), max_model_xyz));									
					
					//Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);					
					//Vector3 mm_uvw; Matrix3x3::Product(yz_rotation_i, min_model_uvw, mm_uvw);					
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, rotation);
					Vector3 mmuvw; Matrix3x3::Product(yz_rotation, min_model_uvw_br, mmuvw);
					float diff = Maths::FAbs(mmuvw.X - min_model_uvw.X) + Maths::FAbs(mmuvw.Y - min_model_uvw.Y) + Maths::FAbs(mmuvw.Z - min_model_uvw.Z);
					if (diff > 0.001f) 
					{
						//std::cout << "Not Equal !! Computed MMUVW : " << mmuvw.ToString() << " Stored MMUVW : " << min_model_uvw.ToString() << std::endl;
						yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);
					}

					Matrix3x3 rot_projection = uvw_inverse * yz_rotation;										

					Vector3 centreShift = Vector3(grid_cell_spacing_u/2, grid_cell_spacing_v/2, grid_cell_spacing_w/2);
					Vector3 topShift = Vector3(grid_cell_spacing_u, grid_cell_spacing_v, grid_cell_spacing_w);

					int discarded_spheres = 0;					
					Vector3 g_pos_xyz;
					float sphere_spacing = 0.2f;
					int sphere_count_u = (int)((float)grid_cell_spacing_u / sphere_spacing);
					int sphere_count_v = (int)((float)grid_cell_spacing_v / sphere_spacing);
					int sphere_count_w = (int)((float)grid_cell_spacing_w / sphere_spacing);

					for (auto voxels_it = m_voxels->begin(); voxels_it != m_voxels->end(); voxels_it++)
					{								

						Vector3 grid_xyz(	(GetXFromKey(voxels_it->first))*grid_cell_spacing_u+(min_model_uvw_br.X),
											(GetYFromKey(voxels_it->first))*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(GetZFromKey(voxels_it->first))*grid_cell_spacing_w+(min_model_uvw_br.Z));

						Vector3 grid_xyz_top((GetXFromKey(voxels_it->first))*grid_cell_spacing_u+(min_model_uvw_br.X),
											(GetYFromKey(voxels_it->first))*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(GetZFromKey(voxels_it->first))*grid_cell_spacing_w+(min_model_uvw_br.Z));
						grid_xyz_top+=topShift;

						if (voxels_it->second.active)
						{
							for (int i = 0; i<sphere_count_u; i++)
							{
								Vector3 point = Vector3(grid_xyz.X+(sphere_spacing*i), grid_xyz.Y, grid_xyz.Z);								
								Matrix3x3::Product(rot_projection, point, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r = std::pair<int, float>(1, 0.10f);
								if (i==0) r.second = 0.25f;
								p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

								Vector3 point2 = Vector3(grid_xyz_top.X-(sphere_spacing*i), grid_xyz_top.Y, grid_xyz_top.Z);								
								Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r2 = std::pair<int, float>(1, 0.10f);
								if (i==0) r2.second = 0.25f;
								p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
							}

							for (int i = 1; i<sphere_count_v; i++)
							{
								Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y+(sphere_spacing*i), grid_xyz.Z);								
								Matrix3x3::Product(rot_projection, point, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r = std::pair<int, float>(1, 0.10f);
								if (i==0) r.second = 0.25f;
								p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

								Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y-(sphere_spacing*i), grid_xyz_top.Z);								
								Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r2 = std::pair<int, float>(1, 0.10f);
								if (i==0) r2.second = 0.25f;
								p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
							}

							for (int i = 1; i<sphere_count_w; i++)
							{
								Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y, grid_xyz.Z+(sphere_spacing*i));								
								Matrix3x3::Product(rot_projection, point, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r = std::pair<int, float>(1, 0.10f);
								if (i==0) r.second = 0.25f;
								p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

								Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y, grid_xyz_top.Z-(sphere_spacing*i));	
								Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
								g_pos_xyz+=o_model_xyz;								
								std::pair<int, float> r2 = std::pair<int, float>(1, 0.10f);
								if (i==0) r2.second = 0.25f;
								p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));								
							}
						}
						else
						{
							if (voxels_it->second.inModel)
							{
								/*
								for (int i = 0; i<sphere_count_u; i++)
								{
									Vector3 point = Vector3(grid_xyz.X+(sphere_spacing*i), grid_xyz.Y, grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									//Vector3 point2 = Vector3(grid_xyz_top.X-(sphere_spacing*i), grid_xyz_top.Y, grid_xyz_top.Z);								
									//Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									//g_pos_xyz+=o_model_xyz;								
									//std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									//if (i==0) r2.second = 0.25f;
									//p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
									
								}

								for (int i = 1; i<sphere_count_v; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y+(sphere_spacing*i), grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									//Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y-(sphere_spacing*i), grid_xyz_top.Z);								
									//Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									//g_pos_xyz+=o_model_xyz;								
									//std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									//if (i==0) r2.second = 0.25f;
									//p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));									
								}

								for (int i = 1; i<sphere_count_w; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y, grid_xyz.Z+(sphere_spacing*i));								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									//Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y, grid_xyz_top.Z-(sphere_spacing*i));	
									//Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									//g_pos_xyz+=o_model_xyz;								
									//std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									//if (i==0) r2.second = 0.25f;
									//p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));								
									
								}*/
							}
							else
							{
								for (int i = 0; i<sphere_count_u; i++)
								{
									Vector3 point = Vector3(grid_xyz.X+(sphere_spacing*i), grid_xyz.Y, grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									Vector3 point2 = Vector3(grid_xyz_top.X-(sphere_spacing*i), grid_xyz_top.Y, grid_xyz_top.Z);								
									Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									if (i==0) r2.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
									
								}

								for (int i = 1; i<sphere_count_v; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y+(sphere_spacing*i), grid_xyz.Z);								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y-(sphere_spacing*i), grid_xyz_top.Z);								
									Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									if (i==0) r2.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));
									
								}

								for (int i = 1; i<sphere_count_w; i++)
								{
									Vector3 point = Vector3(grid_xyz.X, grid_xyz.Y, grid_xyz.Z+(sphere_spacing*i));								
									Matrix3x3::Product(rot_projection, point, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r = std::pair<int, float>(2, 0.10f);
									if (i==0) r.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));

									
									Vector3 point2 = Vector3(grid_xyz_top.X, grid_xyz_top.Y, grid_xyz_top.Z-(sphere_spacing*i));	
									Matrix3x3::Product(rot_projection, point2, g_pos_xyz);
									g_pos_xyz+=o_model_xyz;								
									std::pair<int, float> r2 = std::pair<int, float>(2, 0.10f);
									if (i==0) r2.second = 0.25f;
									p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r2, g_pos_xyz));								
									
								}
							}
						}

						//grid_xyz+=centreShift;
						/*
						for (int i = 0; i<sphere_count_u; i++)
						{
							Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);
							g_pos_xyz+=o_model_xyz;
						    
							std::pair<int, float> r = std::pair<int, float>(1, 0.2f);
						
							if (voxels_it->second.active) r.second = 0.5f;
							if (voxels_it->second.inModel) r.first = 2;
							p_activeCellLocations.push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));
						}
						*/
					}
				}

				void TransformModelVertices(List<Vertex> &vertices, Vector3 p_modelShift, int p_grid_idx, Vector3 p_u, Vector3 p_v, Vector3 p_w)
				{								
					Matrix3x3 uvw_inverse_m(p_u, p_v, p_w);

					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;					
										
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, rotation);
					Vector3 mmuvw; Matrix3x3::Product(yz_rotation, min_model_uvw_br, mmuvw);
					float diff = Maths::FAbs(mmuvw.X - min_model_uvw.X) + Maths::FAbs(mmuvw.Y - min_model_uvw.Y) + Maths::FAbs(mmuvw.Z - min_model_uvw.Z);
					if (diff > 0.001f) yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);
					
					Matrix3x3 xy_rotation = Matrix3x3::CreateRotation(Vector3::UnitZPos, 0);
					Matrix3x3 xz_rotation = Matrix3x3::CreateRotation(Vector3::UnitYNeg, 0); 					
								
					Matrix3x3 rot_projection = uvw_inverse * yz_rotation * xy_rotation * xz_rotation;	
						
					Matrix3x3 rot_projection_m = uvw_inverse_m;
					Vector3 g_pos_xyz;
					Vector3 g_pos_xyz_m;
					Vector3 pos;
					for (int i=0; i<vertices.Size(); i++)
					{
						pos = vertices.At(i).Position;
						pos -= p_modelShift;
						Matrix3x3::Product(rot_projection_m, pos, g_pos_xyz_m);
						Matrix3x3::Product(rot_projection, g_pos_xyz_m, g_pos_xyz);
						g_pos_xyz+=o_model_xyz;
						vertices.At(i).Position.X = g_pos_xyz.X;
						vertices.At(i).Position.Y = g_pos_xyz.Y;
						vertices.At(i).Position.Z = g_pos_xyz.Z;
					}															
				}

				void ScaleToGridModelVertices(List<Vertex> &vertices, Vector3 p_tgrid_origin, int p_grid_idx, Vector3 p_u, Vector3 p_v, Vector3 p_w)
				{								
					Matrix3x3 uvw_inverse_m(p_u, p_v, p_w);

					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
															
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, rotation);
					Vector3 mmuvw; Matrix3x3::Product(yz_rotation, min_model_uvw_br, mmuvw);
					float diff = Maths::FAbs(mmuvw.X - min_model_uvw.X) + Maths::FAbs(mmuvw.Y - min_model_uvw.Y) + Maths::FAbs(mmuvw.Z - min_model_uvw.Z);
					if (diff > 0.001f) yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);
					
					Matrix3x3 xy_rotation = Matrix3x3::CreateRotation(Vector3::UnitZPos, 0);
					Matrix3x3 xz_rotation = Matrix3x3::CreateRotation(Vector3::UnitYNeg, 0); 					
								
					Matrix3x3 rot_projection = uvw_inverse * yz_rotation * xy_rotation * xz_rotation;	
						
					Matrix3x3 rot_projection_m = uvw_inverse_m;
					Vector3 g_pos_xyz;
					Vector3 g_pos_xyz_m;
					Vector3 pos;
					std::cout << "Shifting by " << p_tgrid_origin.ToString();
					for (int i=0; i<vertices.Size(); i++)
					{
						pos = vertices.At(i).Position;
						pos -= p_tgrid_origin;
						Matrix3x3::Product(rot_projection_m, pos, g_pos_xyz_m);
						g_pos_xyz_m *= Vector3(grid_cell_spacing_u, grid_cell_spacing_v, grid_cell_spacing_w);
						Matrix3x3::Product(rot_projection, g_pos_xyz_m, g_pos_xyz);
						g_pos_xyz+=o_model_xyz;
						vertices.At(i).Position.X = g_pos_xyz.X;
						vertices.At(i).Position.Y = g_pos_xyz.Y;
						vertices.At(i).Position.Z = g_pos_xyz.Z;
					}															
				}				

				std::vector<std::pair<std::pair<bool, float>, Vector3>> * GetAllCells()
				{
					std::vector<std::pair<std::pair<bool, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<bool, float>, Vector3>>();
					
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();					
					/*Vector3 min_obb_xyz; Vector3 max_obb_xyz;
					Matrix3x3::Product(uvw_inverse, min_obb_uvw, min_obb_xyz);
					Matrix3x3::Product(uvw_inverse, max_obb_uvw, max_obb_xyz);
					min_obb_xyz+=o_model_xyz;
					max_obb_xyz+=o_model_xyz;
					*/
					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					float radius = grid_cell_spacing_u/16;
					//Include in point set centre, min and max of OBB
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(0, 1.2f), o_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(0, grid_cell_spacing_u*1.2f), min_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<int, float>, Vector3>(std::pair<int, float>(1, grid_cell_spacing_u*1.6f), max_model_xyz));
					//activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, 0.5), min_obb_xyz));
					//activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, 0.5), max_obb_xyz));
					
					//std::cout << "gridcell spacing u:" << grid_cell_spacing_u << " v:" << grid_cell_spacing_v << " w:" << grid_cell_spacing_w << std::endl;
					
					Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(Vector3::UnitXPos, -rotation);
					Vector3 mm_uvw; Matrix3x3::Product(yz_rotation_i, min_model_uvw, mm_uvw);

					mm_uvw = (max_model_xyz - min_model_xyz);
					mm_uvw.Normalize();

					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(Vector3::UnitXPos, rotation);

					Matrix3x3 rot_projection = uvw_inverse;// * yz_rotation;
						
					int discarded_spheres = 0;
					Vector3 g_pos_rot;
					Vector3 g_pos_xyz;
					for (int u=0; u<u_cells_count; u++)
					{						
						for (int v=0; v<v_cells_count; v++)
						{
							for (int w=0; w<w_cells_count; w++)
							{								
								auto voxel = m_voxels->find(GetKey(u,v,w));
								if ((u==0 | u==u_cells_count-1 | v==0 | v==v_cells_count-1 | w==0 | w==w_cells_count-1) | ( u==o_grid_u && v == o_grid_v && w == o_grid_w) |
								     voxel != m_voxels->end()) 
								{
									
									Vector3 grid_xyz(	u*grid_cell_spacing_u+(mm_uvw.X), 
														v*grid_cell_spacing_v+(mm_uvw.Y),
														w*grid_cell_spacing_w+(mm_uvw.Z));

									grid_xyz += min_model_xyz;

									std::pair<bool, float> r = std::pair<int, float>(true, 0.1f);									
									if (u==o_grid_u) { if (v==o_grid_v) { if (w==o_grid_w) { r.first=false; } } }		    //Change the sphere colour of the grid segment origin.
									if (voxel != m_voxels->end()) 
									{ 
										r.first=false; r.second=0.3f;														//Change colour and radius of active cells.
										if (voxel->second.active) { r.second = 0.8f; }										//Inc size if there's an intersection with grid of model
									}							
																				
									//Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);
									//g_pos_xyz+=o_model_xyz;
									g_pos_xyz = grid_xyz;
									activeCellsLocations->push_back(std::pair<std::pair<int, float>,Vector3>(r, g_pos_xyz));
								} else
								{									
									discarded_spheres ++;
								}
							}
						}
					}
					//std::cout << "Discarded Spheres: " << discarded_spheres << std::endl;
					return activeCellsLocations;
				}

				std::vector<Vector3> GetBasisVectorsInWorldSpace()
				{
					std::vector<Vector3> bases;
					
					//std::vector<std::pair<std::pair<bool, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<bool, float>, Vector3>>();
					
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();					
					Vector3 min_obb_xyz; Vector3 max_obb_xyz;
					Matrix3x3::Product(uvw_inverse, min_obb_uvw, min_obb_xyz);
					Matrix3x3::Product(uvw_inverse, max_obb_uvw, max_obb_xyz);
					min_obb_xyz+=o_model_xyz;
					max_obb_xyz+=o_model_xyz;

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					//float radius = grid_cell_spacing_u/8;
					
					//activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), o_model_xyz));
					//activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), min_model_xyz));
					//activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(true, grid_cell_spacing_u/2), max_model_xyz));					
										
					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(uvw.U, rotation);

					Matrix3x3 rot_projection = yz_rotation * uvw_inverse;

					float spacing = 2.0f;
						
					int discarded_spheres = 0;
					Vector3 g_pos_rot;

					//std::cout << "Min_Model_uvw_br::" << min_model_uvw_br.ToString() << std::endl;

					Vector3 g_pos_xyz;
					Vector3 grid_xyz(	0*spacing+(min_model_uvw_br.X),
										0*spacing+(min_model_uvw_br.Y),
										0*spacing+(min_model_uvw_br.Z));	
					//std::cout << "Grid_xyz::" << grid_xyz.ToString() << std::endl;
																						
					Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);
					//std::cout << "Grid_xyz_R::" << g_pos_xyz.ToString() << std::endl;
					g_pos_xyz+=o_model_xyz;					
					bases.push_back(g_pos_xyz);

					Vector3 g_pos_xyz1;					
					Vector3 grid_xyz1(	1*spacing+(min_model_uvw_br.X),
										0*spacing+(min_model_uvw_br.Y),
										0*spacing+(min_model_uvw_br.Z));	
																						
					Matrix3x3::Product(rot_projection, grid_xyz1, g_pos_xyz1);
					g_pos_xyz1+=o_model_xyz;					
					bases.push_back(g_pos_xyz1);

					Vector3 g_pos_xyz2;					
					Vector3 grid_xyz2(	0*spacing+(min_model_uvw_br.X),
										1*spacing+(min_model_uvw_br.Y),
										0*spacing+(min_model_uvw_br.Z));	
																						
					Matrix3x3::Product(rot_projection, grid_xyz2, g_pos_xyz2);
					g_pos_xyz2+=o_model_xyz;					
					bases.push_back(g_pos_xyz2);

					Vector3 g_pos_xyz3;		
					Vector3 grid_xyz3(	0*spacing+(min_model_uvw_br.X),
										0*spacing+(min_model_uvw_br.Y),
										1*spacing+(min_model_uvw_br.Z));	
																						
					Matrix3x3::Product(rot_projection, grid_xyz3, g_pos_xyz3);
					g_pos_xyz3+=o_model_xyz;					
					bases.push_back(g_pos_xyz3);

					return bases;									
				}

				std::vector<std::pair<std::pair<bool, float>, Vector3>> * GetActiveOverlappingCellsWithGridBoundaries(VoxelGrid * _targetGrid)
				{
					std::vector<std::pair<std::pair<bool, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<bool, float>, Vector3>>();
					
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();					
					Vector3 min_obb_xyz; Vector3 max_obb_xyz;
					Matrix3x3::Product(uvw_inverse, min_obb_uvw, min_obb_xyz);
					Matrix3x3::Product(uvw_inverse, max_obb_uvw, max_obb_xyz);
					min_obb_xyz+=o_model_xyz;
					max_obb_xyz+=o_model_xyz;

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					float radius = grid_cell_spacing_u/8;
					
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), o_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), min_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(true, grid_cell_spacing_u/2), max_model_xyz));															

					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(uvw.U, rotation);

					Matrix3x3 rot_projection = yz_rotation * uvw_inverse;
						
					int discarded_spheres = 0;
					
					Vector3 g_pos_xyz;
					for (int u=0; u<u_cells_count; u++)
					{						
						for (int v=0; v<v_cells_count; v++)
						{
							for (int w=0; w<w_cells_count; w++)
							{								
								auto voxel = m_voxels->find(GetKey(u,v,w));
								if ((u==0 | u==u_cells_count-1 | v==0 | v==v_cells_count-1 | w==0 | w==w_cells_count-1) | ( u==o_grid_u && v == o_grid_v && w == o_grid_w) |
								     voxel != m_voxels->end()) 
								{
									Vector3 grid_xyz(	u*grid_cell_spacing_u+(min_model_uvw_br.X),
														v*grid_cell_spacing_v+(min_model_uvw_br.Y),
														w*grid_cell_spacing_w+(min_model_uvw_br.Z));	

									std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
									if (voxel != m_voxels->end())
										if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

									if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

									Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

									g_pos_xyz+=o_model_xyz;
									activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));
								} else
								{									
									discarded_spheres ++;
								}
							}
						}
					}
					std::cout << "Discarded Spheres: " << discarded_spheres << std::endl;
					return activeCellsLocations;
				}

				std::vector<std::pair<std::pair<bool, float>, Vector3>> * GetActiveOverlappingCellsWithGridBoundaries2(VoxelGrid * _targetGrid)
				{
					std::vector<std::pair<std::pair<bool, float>, Vector3>> * activeCellsLocations = new std::vector<std::pair<std::pair<bool, float>, Vector3>>();
					
					//Compute the inverse of uvw -> to get us back to world coordinates. Rotate -> Translate
					Matrix3x3 uvw_inverse = uvw.GetMatrix();
					uvw_inverse.Transpose();					
					Vector3 min_obb_xyz; Vector3 max_obb_xyz;
					Matrix3x3::Product(uvw_inverse, min_obb_uvw, min_obb_xyz);
					Matrix3x3::Product(uvw_inverse, max_obb_uvw, max_obb_xyz);
					min_obb_xyz+=o_model_xyz;
					max_obb_xyz+=o_model_xyz;

					Vector3 min_model_xyz; Vector3 max_model_xyz;
					Matrix3x3::Product(uvw_inverse, min_model_uvw, min_model_xyz);
					Matrix3x3::Product(uvw_inverse, max_model_uvw, max_model_xyz);
					min_model_xyz+=o_model_xyz;
					max_model_xyz+=o_model_xyz;
					
					float radius = grid_cell_spacing_u/8;
					
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), o_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(false, grid_cell_spacing_u/2), min_model_xyz));
					activeCellsLocations->push_back(std::pair<std::pair<bool, float>, Vector3>(std::pair<bool, float>(true, grid_cell_spacing_u/2), max_model_xyz));															

					Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(uvw.U, rotation);

					Matrix3x3 rot_projection = yz_rotation * uvw_inverse;
						
					int discarded_spheres = 0;
					
					Vector3 g_pos_xyz;
					
					for (int u=0; u<u_cells_count; u++) {											
						auto voxel = m_voxels->find(GetKey(u,0,0));
						Vector3 grid_xyz(	u*grid_cell_spacing_u+(min_model_uvw_br.X),
											0*grid_cell_spacing_v+(min_model_uvw_br.Y),
											0*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int v=0; v<v_cells_count; v++) {											
						auto voxel = m_voxels->find(GetKey(0,v,0));
						Vector3 grid_xyz(	0*grid_cell_spacing_u+(min_model_uvw_br.X),
											v*grid_cell_spacing_v+(min_model_uvw_br.Y),
											0*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int w=0; w<w_cells_count; w++) {											
						auto voxel = m_voxels->find(GetKey(0,0,w));
						Vector3 grid_xyz(	0*grid_cell_spacing_u+(min_model_uvw_br.X),
											0*grid_cell_spacing_v+(min_model_uvw_br.Y),
											w*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int u=0; u<u_cells_count; u++) {											
						auto voxel = m_voxels->find(GetKey(u,v_cells_count-1,w_cells_count-1));
						Vector3 grid_xyz(	u*grid_cell_spacing_u+(min_model_uvw_br.X),
											(v_cells_count-1)*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(w_cells_count-1)*grid_cell_spacing_w+(min_model_uvw_br.Z));

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int u=0; u<u_cells_count; u++) {											
						auto voxel = m_voxels->find(GetKey(u,0,w_cells_count-1));
						Vector3 grid_xyz(	u*grid_cell_spacing_u+(min_model_uvw_br.X),
											0*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(w_cells_count-1)*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int u=0; u<u_cells_count; u++) {											
						auto voxel = m_voxels->find(GetKey(u,v_cells_count-1,0));
						Vector3 grid_xyz(	u*grid_cell_spacing_u+(min_model_uvw_br.X),
											(v_cells_count-1)*grid_cell_spacing_v+(min_model_uvw_br.Y),
											0*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					
					for (int v=0; v<v_cells_count; v++) {											
						auto voxel = m_voxels->find(GetKey(0,v,w_cells_count-1));
						Vector3 grid_xyz(	0*grid_cell_spacing_u+(min_model_uvw_br.X),
											v*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(w_cells_count-1)*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int w=0; w<w_cells_count; w++) {											
						auto voxel = m_voxels->find(GetKey(0,v_cells_count-1,w));
						Vector3 grid_xyz(	0*grid_cell_spacing_u+(min_model_uvw_br.X),
											(v_cells_count-1)*grid_cell_spacing_v+(min_model_uvw_br.Y),
											w*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}
					
					for (int w=0; w<w_cells_count; w++) {											
						auto voxel = m_voxels->find(GetKey(u_cells_count-1,v_cells_count-1,w));
						Vector3 grid_xyz(	(u_cells_count-1)*grid_cell_spacing_u+(min_model_uvw_br.X),
											(v_cells_count-1)*grid_cell_spacing_v+(min_model_uvw_br.Y),
											w*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int v=0; v<v_cells_count; v++) {
						auto voxel = m_voxels->find(GetKey(u_cells_count-1,v,w_cells_count-1));
						Vector3 grid_xyz(	(u_cells_count-1)*grid_cell_spacing_u+(min_model_uvw_br.X),
											v*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(w_cells_count-1)*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int w=0; w<w_cells_count; w++) {											
						auto voxel = m_voxels->find(GetKey(u_cells_count-1,0,w));
						Vector3 grid_xyz(	(u_cells_count-1)*grid_cell_spacing_u+(min_model_uvw_br.X),
											0*grid_cell_spacing_v+(min_model_uvw_br.Y),
											w*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}

					for (int v=0; v<v_cells_count; v++) {											
						auto voxel = m_voxels->find(GetKey(u_cells_count-1,v,0));
						Vector3 grid_xyz(	(u_cells_count-1)*grid_cell_spacing_u+(min_model_uvw_br.X),
											v*grid_cell_spacing_v+(min_model_uvw_br.Y),
											0*grid_cell_spacing_w+(min_model_uvw_br.Z));	

						std::pair<bool, float> r = std::pair<bool, float>(true, radius);
																		
						if (voxel != m_voxels->end())
							if (_targetGrid->m_voxels->find(voxel->first) == _targetGrid->m_voxels->end()) continue;	//No overlap

						if (voxel != m_voxels->end()) { r.first=false; r.second*=1.2f; }								//Change colour and radius of active cells.

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);

						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));						
					}
					


					for (auto voxels_it = m_voxels->begin(); voxels_it != m_voxels->end(); voxels_it++)
					{								
						Vector3 grid_xyz(	(GetXFromKey(voxels_it->first))*grid_cell_spacing_u+(min_model_uvw_br.X), 
											(GetYFromKey(voxels_it->first))*grid_cell_spacing_v+(min_model_uvw_br.Y),
											(GetZFromKey(voxels_it->first))*grid_cell_spacing_w+(min_model_uvw_br.Z));

						std::pair<bool, float> r = std::pair<bool, float>(false, radius);			

						Matrix3x3::Product(rot_projection, grid_xyz, g_pos_xyz);
						g_pos_xyz+=o_model_xyz;
						activeCellsLocations->push_back(std::pair<std::pair<bool, float>,Vector3>(r, g_pos_xyz));														
					}
										
					return activeCellsLocations;
				}

				std::pair<int, std::pair<float, float>> ComputeOverlapDiffWithGridUVW(VoxelGrid * _sceneGrid) 
				{
					//std::cout << "Comparing Model VG size[" << m_voxels->size() << "] with Scene VG size[" << _sceneGrid->m_voxels->size() << "]" << std::endl;
					int key;
					int pos_matches = 0;
					int neg_matches = 0;
					int no_match = 0;
					bool present_in_training_grid;
					bool present_in_scene_grid;				

					for (int u=0; u<u_cells_count; u++)
					{						
						for (int v=0; v<v_cells_count; v++)
						{
							for (int w=0; w<w_cells_count; w++)
							{
								key = GetKey(u, v, w);
								m_voxels->find(key) == m_voxels->end() ? present_in_training_grid = false : present_in_training_grid = true;
								_sceneGrid->m_voxels->find(key) == _sceneGrid->m_voxels->end() ? present_in_scene_grid = false : present_in_scene_grid = true;
								if (present_in_training_grid && present_in_scene_grid) { pos_matches++; continue; }
								if (!present_in_training_grid && !present_in_scene_grid) { neg_matches++; continue; }
								no_match++;
							}
						}
					}

					return std::pair<int, std::pair<float, float>>(no_match, std::pair<float, float>(pos_matches, neg_matches));
				}				

				//Added functionality for history viewing. Remove when not needed.
				inline void ComputeOverlapPosWithGridUVW(VoxelGrid * _sceneGrid, int p_totalPointsInDownsampledSegs, int p_segsCount, Match_Result &result) 
				{
					result.Reset();					
					int match_count = 0;					
					bool inOrigin=false;
					int new_cells = 0;
					int voxels_in_scene = _sceneGrid->m_voxels->size();
					for (auto voxels_it = m_voxels->begin(); voxels_it != m_voxels->end(); voxels_it++)
					{
						auto scene_voxel_it = _sceneGrid->m_voxels->find(voxels_it->first);
						inOrigin=false; if (voxels_it->second.inOrigin) { result.in_anchor_model++; inOrigin=true; } else result.not_in_anchor_model++;
						if (scene_voxel_it != _sceneGrid->m_voxels->end())
						{
							if (scene_voxel_it->second.inModel == false)
							{
								scene_voxel_it->second.active = true;
								match_count++;	//This voxel is active in the scene as well
								if (scene_voxel_it->second.inOrigin)
								{
									result.in_anchor_scene++;
									if (inOrigin) result.anchor_matches++;
								} 
								else
								{
									if (!inOrigin) 
									{
										result.not_in_anchor_matches++;
										//std::cout << voxels_it->second.u_pos << voxels_it->second.v_pos << voxels_it->second.w_pos << std::endl;
										//std::cout << scene_voxel_it->second.u_pos << scene_voxel_it->second.v_pos << scene_voxel_it->second.w_pos << std::endl;
									}
								}
								result.segments_used.find(scene_voxel_it->second.segId) == result.segments_used.end() ? 
																							result.segments_used[scene_voxel_it->second.segId] = 1 : result.segments_used[scene_voxel_it->second.segId]++; 
							}
						}
						else
						{							
							VoxelGridCell vgc(voxels_it->second);
							vgc.inOrigin = false;
							vgc.inModel = true;
							new_cells++;
							(*(_sceneGrid->m_voxels))[voxels_it->first] = vgc;
						}
					}

					int points_out = _sceneGrid->above_u_max + _sceneGrid->below_u_min +
                                        _sceneGrid->below_v_min + _sceneGrid->above_v_max +
                                        _sceneGrid->below_w_min + _sceneGrid->above_w_max;					

					if (p_segsCount == 1) result.inclusion_bias = 2.0f; 
					else points_out == 0 ? result.inclusion_bias = 0.f : result.inclusion_bias = (float)points_out*2.5f / (float)p_totalPointsInDownsampledSegs;
                                                
                    result.coverage -= result.inclusion_bias;								//Promote matches which include the most points in the scene.
						
                    int inAnchorVoxels = _sceneGrid->GetVoxelCountInOrigin();
                    int anchor_diff = result.in_anchor_model - result.anchor_matches;	//Promote matches which are very close to the anchor segment - model -> scene
					anchor_diff = Maths::Max(anchor_diff, inAnchorVoxels - result.anchor_matches);
					if (anchor_diff == 0) anchor_diff++;
						
					//anchor_diff < 2 ? result.anchor_match_bias = 0.8f : result.anchor_match_bias = 1 / Maths::Sqrt(anchor_diff);						                    
					result.anchor_match_bias = 1 - Maths::Log(anchor_diff) / Maths::Log(result.in_anchor_model);
					result.coverage += result.anchor_match_bias;

																						//Promote matches which include more than just the anchor segment.
					int non_anchor_diff = result.not_in_anchor_matches;
					if (non_anchor_diff == 0) non_anchor_diff++;
					//result.not_in_anchor_matches == 0 ? result.non_anchor_match_bias = 0.f : result.non_anchor_match_bias = 1 - (1/Maths::Sqrt(result.not_in_anchor_matches));			
					result.non_anchor_match_bias = Maths::Log(non_anchor_diff) / Maths::Log(_sceneGrid->m_voxels->size() - inAnchorVoxels);
					result.coverage += result.non_anchor_match_bias;                        
           
					result.segment_inclusion_bias = 1; result.segment_inclusion_bias += 0.05f * p_segsCount;
					result.coverage *= result.segment_inclusion_bias;

					if (new_cells == 0) new_cells++;
					result.coverage = ( ((float)1 / (float)new_cells) - result.inclusion_bias);// * (float)(result.segments_used.size());
					result.new_cells = new_cells;

					result.matches = match_count;
				}

				inline int ComputeAnchorDistanceWithGridUVW(VoxelGrid * _sceneGrid) 
				{								
					int match_count_0 = 0;
					int match_count_1 = 0;
					for (auto voxels_it = m_voxels->begin(); voxels_it != m_voxels->end(); voxels_it++)
					{
						if (voxels_it->second.inOrigin)
						{
							auto scene_voxel_it = _sceneGrid->m_voxels->find(voxels_it->first);
							if (scene_voxel_it != _sceneGrid->m_voxels->end())
							{
								if (scene_voxel_it->second.inOrigin == false) match_count_0++;
							}
							else
							{
								match_count_0++;
							}
						}
					}					

					for (auto voxels_it = _sceneGrid->m_voxels->begin(); voxels_it != _sceneGrid->m_voxels->end(); voxels_it++)
					{
						if (voxels_it->second.inModel) continue;
						if (voxels_it->second.inOrigin)
						{
							auto model_voxel_it = m_voxels->find(voxels_it->first);
							if (model_voxel_it != m_voxels->end())
							{
								if (model_voxel_it->second.inOrigin == false) match_count_1++;
							}
							else
							{
								match_count_1++;
							}
						}
					}
					//std::cout << "Computing Distance: " << match_count_0 << ":" << match_count_1 << std::endl;
					return Maths::Max(match_count_0, match_count_1);
				}

				void Reset(int p_u_cells_count, int p_v_cells_count, int p_w_cells_count)
				{
					u_cells_count = p_u_cells_count;
					v_cells_count = p_v_cells_count;
					w_cells_count = p_w_cells_count;
					above_u_max = 0;
					below_u_min = 0;
					above_v_max = 0;
					below_v_min = 0;
					above_w_max = 0;
					below_w_min = 0;
					points_total = 0;
					dist_above_u_max = 0;
					dist_below_u_min = 0;
					dist_above_v_max = 0;
					dist_below_v_min = 0;
					dist_above_w_max = 0;
					dist_below_w_min = 0;
					scale = Vector3(1);
					rotation = 0;
					m_voxels->clear();
				}

				std::string ToString(void) const
				{
					std::string strOut = boost::str(boost::format("[grid_cell_spacing = %d %d %d, cell_count = %d %d %d, MinUVW = %d %d %d, MinUVW_BR = %d %d %d, UVW = (%d %d %d, %d %d %d, %d %d %d), Rot = %f]") 
						% this->grid_cell_spacing_u
						% this->grid_cell_spacing_v
						% this->grid_cell_spacing_w
						% this->u_cells_count
						% this->v_cells_count
						% this->w_cells_count
						% this->min_model_uvw.X
						% this->min_model_uvw.Y
						% this->min_model_uvw.Z
						% this->min_model_uvw_br.X
						% this->min_model_uvw_br.Y
						% this->min_model_uvw_br.Z
						% this->uvw.U.X
						% this->uvw.U.Y
						% this->uvw.U.Z
						% this->uvw.V.X
						% this->uvw.V.Y
						% this->uvw.V.Z
						% this->uvw.W.X
						% this->uvw.W.Y
						% this->uvw.W.Z
						% this->rotation
						);
				
					return strOut;
				}

				int sizeInBytes()
				{																	
					int s = 0;
					s += sizeof(float) * 3;								//grid_cell_spacing_u, grid_cell_spacing_v, grid_cell_spacing_w
					s += sizeof(int) * 3;								//u_cells_count, v_cells_count, w_cells_count
					s += sizeof(int);									//points_total
					s += sizeof(int) * 3;								//o_grid_u, o_grid_v, o_grid_w
					s += sizeof(float) * 9;								//uvw
					s += sizeof(float) * 3;								//o_model_xyz
					s += sizeof(float) * 3;								//min_model_uvw
					s += sizeof(float) * 3;								//max_model_uvw
					s += sizeof(float) * 3;								//min_obb_uvw
					s += sizeof(float) * 3;								//max_obb_uvw
					s += sizeof(float) * 3;								//min_model_uvw_br
					s += sizeof(float) * 3;								//scale
					s += sizeof(float);									//rotation

					s += sizeof(int);									//store number of voxels
					for (auto v : *m_voxels) s += sizeof(int) * 6;		//Key + number of surface points, number of edge points and origin.
					return s;
				}

				void CopyFrom(VoxelGrid &p_vg)
				{
					m_parent = p_vg.m_parent;
					u_cells_count = p_vg.u_cells_count;
					v_cells_count = p_vg.v_cells_count;
					w_cells_count = p_vg.w_cells_count;
					scale = p_vg.scale;
					rotation = p_vg.rotation;
					points_total = p_vg.points_total;

					idx = p_vg.idx;

					grid_cell_spacing_u = p_vg.grid_cell_spacing_u;
					grid_cell_spacing_v = p_vg.grid_cell_spacing_v;
					grid_cell_spacing_w = p_vg.grid_cell_spacing_w;
						
					min_model_uvw.X = p_vg.min_model_uvw.X;
					min_model_uvw.Y = p_vg.min_model_uvw.Y;
					min_model_uvw.Z = p_vg.min_model_uvw.Z;
						
					min_model_uvw_br.X = p_vg.min_model_uvw_br.X;
					min_model_uvw_br.Y = p_vg.min_model_uvw_br.Y;
					min_model_uvw_br.Z = p_vg.min_model_uvw_br.Z;

					max_model_uvw.X = p_vg.max_model_uvw.X;
					max_model_uvw.Y = p_vg.max_model_uvw.Y;
					max_model_uvw.Z = p_vg.max_model_uvw.Z;										

					o_model_xyz.X = p_vg.o_model_xyz.X;
					o_model_xyz.Y = p_vg.o_model_xyz.Y;
					o_model_xyz.Z = p_vg.o_model_xyz.Z;
						
					uvw.U.X = p_vg.uvw.U.X;
					uvw.U.Y = p_vg.uvw.U.Y;
					uvw.U.Z = p_vg.uvw.U.Z;
					uvw.V.X = p_vg.uvw.V.X;
					uvw.V.Y = p_vg.uvw.V.Y;
					uvw.V.Z = p_vg.uvw.V.Z;
					uvw.W.X = p_vg.uvw.W.X;
					uvw.W.Y = p_vg.uvw.W.Y;
					uvw.W.Z = p_vg.uvw.W.Z;

					above_u_max = p_vg.above_u_max;
					below_u_min = p_vg.below_u_min;
					above_v_max = p_vg.above_v_max;
					below_v_min = p_vg.below_v_min;
					above_w_max = p_vg.above_w_max;
					below_w_min = p_vg.below_w_min;

					dist_above_u_max = p_vg.dist_above_u_max;
					dist_below_u_min = p_vg.dist_below_u_min;
					dist_above_v_max = p_vg.dist_above_v_max;
					dist_below_v_min = p_vg.dist_below_v_min;
					dist_above_w_max = p_vg.dist_above_w_max;
					dist_below_w_min = p_vg.dist_below_w_min;
					m_voxels->clear();
					for (auto vox : *(p_vg.m_voxels)) (*(m_voxels))[vox.first] = VoxelGridCell(vox.second);
				}

				VoxelGrid(float p_grid_cell_spacing, FeatureDescStructureGraph * p_parent)
				{
					grid_cell_spacing_u = p_grid_cell_spacing;
					grid_cell_spacing_v = p_grid_cell_spacing;
					grid_cell_spacing_w = p_grid_cell_spacing;
					m_parent = p_parent;
					u_cells_count = 0;
					v_cells_count = 0;
					w_cells_count = 0;
					above_u_max = 0;
					below_u_min = 0;
					above_v_max = 0;
					below_v_min = 0;
					above_w_max = 0;
					below_w_min = 0;
					dist_above_u_max = 0;
					dist_below_u_min = 0;
					dist_above_v_max = 0;
					dist_below_v_min = 0;
					dist_above_w_max = 0;
					dist_below_w_min = 0;
					points_total = 0;
					scale = Vector3(1);
					rotation = 0;
					m_voxels = new std::map<int, VoxelGridCell>();					
				}

				VoxelGrid(int p_u_cells_count, int p_v_cells_count, int p_w_cells_count)
				{					
					m_parent = NULL;
					u_cells_count = p_u_cells_count;
					v_cells_count = p_v_cells_count;
					w_cells_count = p_w_cells_count;
					scale = Vector3(1);
					rotation = 0;
					points_total = 0;
					above_u_max = 0;
					below_u_min = 0;
					above_v_max = 0;
					below_v_min = 0;
					above_w_max = 0;
					below_w_min = 0;
					dist_above_u_max = 0;
					dist_below_u_min = 0;
					dist_above_v_max = 0;
					dist_below_v_min = 0;
					dist_above_w_max = 0;
					dist_below_w_min = 0;
					m_voxels = new std::map<int, VoxelGridCell>();					
				}

				VoxelGrid(VoxelGrid &p_vg)
				{					
					m_parent = NULL;
					u_cells_count = p_vg.u_cells_count;
					v_cells_count = p_vg.v_cells_count;
					w_cells_count = p_vg.w_cells_count;
					scale = p_vg.scale;
					rotation = p_vg.rotation;
					points_total = p_vg.points_total;

					idx = p_vg.idx;

					grid_cell_spacing_u = p_vg.grid_cell_spacing_u;
					grid_cell_spacing_v = p_vg.grid_cell_spacing_v;
					grid_cell_spacing_w = p_vg.grid_cell_spacing_w;
						
					min_model_uvw.X = p_vg.min_model_uvw.X;
					min_model_uvw.Y = p_vg.min_model_uvw.Y;
					min_model_uvw.Z = p_vg.min_model_uvw.Z;
						
					min_model_uvw_br.X = p_vg.min_model_uvw_br.X;
					min_model_uvw_br.Y = p_vg.min_model_uvw_br.Y;
					min_model_uvw_br.Z = p_vg.min_model_uvw_br.Z;

					max_model_uvw.X = p_vg.max_model_uvw.X;
					max_model_uvw.Y = p_vg.max_model_uvw.Y;
					max_model_uvw.Z = p_vg.max_model_uvw.Z;										

					o_model_xyz.X = p_vg.o_model_xyz.X;
					o_model_xyz.Y = p_vg.o_model_xyz.Y;
					o_model_xyz.Z = p_vg.o_model_xyz.Z;
						
					uvw.U.X = p_vg.uvw.U.X;
					uvw.U.Y = p_vg.uvw.U.Y;
					uvw.U.Z = p_vg.uvw.U.Z;
					uvw.V.X = p_vg.uvw.V.X;
					uvw.V.Y = p_vg.uvw.V.Y;
					uvw.V.Z = p_vg.uvw.V.Z;
					uvw.W.X = p_vg.uvw.W.X;
					uvw.W.Y = p_vg.uvw.W.Y;
					uvw.W.Z = p_vg.uvw.W.Z;

					above_u_max = p_vg.above_u_max;
					below_u_min = p_vg.below_u_min;
					above_v_max = p_vg.above_v_max;
					below_v_min = p_vg.below_v_min;
					above_w_max = p_vg.above_w_max;
					below_w_min = p_vg.below_w_min;

					dist_above_u_max = p_vg.dist_above_u_max;
					dist_below_u_min = p_vg.dist_below_u_min;
					dist_above_v_max = p_vg.dist_above_v_max;
					dist_below_v_min = p_vg.dist_below_v_min;
					dist_above_w_max = p_vg.dist_above_w_max;
					dist_below_w_min = p_vg.dist_below_w_min;
					m_voxels = new std::map<int, VoxelGridCell>();
					for (auto vox : *(p_vg.m_voxels)) (*(m_voxels))[vox.first] = VoxelGridCell(vox.second);
				}

				~VoxelGrid()
				{										
					Safe_Delete(m_voxels);
				}
					
			};

			struct VG_stats {
				FeatureDescStructureGraph::VoxelGrid * vgrid;
				FeatureDescStructureGraph::VoxelGrid * tgrid;
				std::vector<int> segs;
				Vector3 best_min_uvw_t;
				Vector3 best_max_uvw_t;
				float best_rot;
				int min_diff;
				int max_pos;
				int max_neg;
				int total_vg_built;
				float max_coverage;
				float max_inclusion_bias;
				int max_anchor_matches;				
				float coverage;
				int grid_idx;
				int non_anchor_matches;
				int max_non_anchor_matches;
				float max_anchor_match_bias;
				float max_non_anchor_match_bias;
				int min_anchor_diff;
				int new_cells;

				VG_stats(FeatureDescStructureGraph::VoxelGrid * _tgrid, int _grid_idx)
					: best_rot(0)
					, vgrid(new FeatureDescStructureGraph::VoxelGrid(1,1,1))
					, segs()
					, tgrid(_tgrid)
					, best_min_uvw_t(0)
					, best_max_uvw_t(0)
					, min_diff(INT_MAX)
					, max_pos(0)
					, max_neg(0)
					, total_vg_built(0)					
					, coverage(0)
					, non_anchor_matches(0)
					, max_non_anchor_matches(0)
					, max_coverage(-1000.f)
					, max_inclusion_bias(0)
					, max_anchor_matches(0)
					, max_anchor_match_bias(0.0f)
					, max_non_anchor_match_bias(0.0f)
					, grid_idx(_grid_idx)
					, min_anchor_diff(10000)
					, new_cells(0)
				{}								

			};

			std::map<int, Node*> * nodes;
			std::map<int, Node*>::iterator nodesIterator;

			std::map<int, std::vector<std::vector<int>>*> * m_stringDescriptors;      //Not very efficient but will do.
			VoxelGrid * m_grid_u_v_w;			
			VoxelGrid * m_grid_u_v_inv_w;
			VoxelGrid * m_grid_u_v_w_inv;
			VoxelGrid * m_grid_u_v_inv_w_inv;

			VoxelGrid * m_grid_u_inv_v_w;
			VoxelGrid * m_grid_u_inv_v_inv_w;
			VoxelGrid * m_grid_u_inv_v_w_inv;			
			VoxelGrid * m_grid_u_inv_v_inv_w_inv;
						
			PointCloudSegmentation * m_pcs;   //pcs instance associated with this feature description....

			int initial_node_id;

			FeatureDescStructureGraph(int pId, PointCloudSegmentation * p_pcs);	//Environment * p_environment);

			void CopyProperties(Node * _n, StructureGraph::Node * str_node);

			void SetInitialNodeId(int pId);
			int GetInitialNodeId(void);
			Node * GetInitialNode();
			Node * GetNode(int _id);
			bool ContainsNode(int pId);

			//Create and populate a new node to represent a surface segment. Add to nodes map.
			void AddNode(Node * p_node);
			Node * AddNode(StructureGraph::Node * p_node);
			Node * AddNode(int p_nodeId);
			void AddConnection(Node * src_node, Node * sink_node, StructureGraph::EdgeStats* _stats);

			std::vector<Node *> GetDirectlyConnectedNodes();
			std::vector<Node *> GetOccludedConnectedNodes();
			std::vector<Node *> GetDisconnectedNodes();

			void ComputeStringDescriptors(int _max_length);
			void ComputeVoxelGrids(float p_voxel_length, std::vector<CloudPoint> &p_pointList);
			VoxelGrid * ComputeVoxelGrid(float p_voxel_length, std::vector<CloudPoint> &p_pointList, bool _invert_u, bool _invert_v, bool _invert_w);			

			std::vector<std::vector<int>> * GetStringDescriptors(int _length);  //return descriptors with string length == _length
			std::pair <float, std::vector<int>> GetStringDescriptorMaxCoverage(int _length);       //return descriptor with max coverage of length _length
			std::pair <float, std::vector<int>> GetStringDescriptorMaxPointCount(int _length);
			std::pair <float, std::vector<int>> GetStringDescriptorMaxCoverageCrossPointCount(int _length);

			//**********************************************
			//**********Serialisation Functions********************
			void WriteToBFDSGFile(std::string _filepath);
			bool ReadFromBFDSGFile(std::string _filepath); 			
			
			void PrintDescriptors(); //0 print all descriptors
		};
	}
}